#ifndef THREED_ELEMENTARY_GEOMETRY_TOOL_LIBRARY_H_
#define THREED_ELEMENTARY_GEOMETRY_TOOL_LIBRARY_H_

#include<stdlib.h>
#include<stdio.h>
#include<math.h>
#include <Matrix/LB_Matrix.h>
#include <Math/LB_Quaternions.h>


#ifndef SIGN
#define SIGN(x) ((x>0)?1:((x<0)?-1:0))
#endif
#ifndef SAFE_SQRT
#define SAFE_SQRT(x) (x>=0?sqrt(x):0)
#endif
#ifndef SAFE_FREE
#define SAFE_FREE(x) if(x!=NULL){free(x);x=NULL;}
#endif
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#ifdef __cplusplus
extern "C" {
#endif

void compute_plane_normal_of_jordan_curve(double** v,int len,double* re );

// return (a,b,c,d) where ax+by+cz+d=0 ,(a,b,c) is normalized vector
// 返回经过三点的平面，不管是否有无数个平面
// 法向归一化了
// @return 1(只有一个平面) 0（无数个平面）

int get_plane_equation_of_triangular_plane(double* tr1,double* tr2,double*tr3,double*re);

//
// 返回经过多边形的平面，不管是否有无数个平面,
// 法向归一化了
// @return 1(只有一个平面) 0（无数个平面）
//
int get_plane_equation_of_polygon(
    double**poly,int len,double*re);

//@ p1 is a plane equation
//@ p2 is a plane equation
//return t* n+ p:: line equation
// 返回尽量靠近两个平面的直线,不管是否有多个
// 两个平面都要求法向归一化
//
// @return 1(只有一个直线) 0（无数个直线）
//
int get_line_equation_from_2planes(double * p1,double *p2,double* re);
//
//返回一个经过两点的线程方程，不管两点是否一样，是否有无数个方程
//
// @return 1(只有一个方程) 0（无数个方程）

static inline int get_line_equation_from_two_points(double*p1,double* p2,double*re )
{
    // double * re=(double*)malloc(sizeof(double)*6);
    re[3]=p1[0];re[4]=p1[1];re[5]=p1[2];

    re[0]=p2[0]-p1[0];re[1]=p2[1]-p1[1];re[2]=p2[2]-p1[2];
    if(!normalize(re,3))
    {
        re[0]=1.0;re[1]=0;re[2]=0;
        // free(re);
        return 0;
    }

    return 1;
    // return re;
}


// @ l: the normalized line n1*t1+p1
// @ l1:the normalized line n2*t2+p2
// @ return the t1 t2
// 返回一个最近一对点，尽管可能有无数个最近点
// @ return 1(一对点) 0（无数个最近点）
int line_point_nearest_2_line(double* l,double* l1,double* t1,double *t2);

//
// l: n*t+p
// return t
static inline double point_nearest_2_line(double* p,double* l)
{
    double fac=inner_product(l,l,3);
    if(double_equal_zero(fac))
    {
        // *t=0;
        return 0;
    }
    return ( (p[0]-l[3])*l[0]+(p[1]-l[4])*l[1]+(p[2]-l[5])*l[2] )/fac;

}

// @ l: the normalized line n1*t1+p1
//
static inline double distance_from_point_to_line(double* p,double* l)
{
    double a[3]={p[0]-l[3],p[1]-l[4],p[2]-l[5]};
    double dis=a[0]*l[0]+a[1]*l[1]+a[2]*l[2];
    double dis1=a[0]*a[0]+a[1]*a[1]+a[2]*a[2];
    return SAFE_SQRT(dis1-dis*dis);
}


// 这里应该抽象出来点到n维单形最近点的算法。
// back_dim 应该大于等于len-1
// len>=1
// @ return 1：这个最近点就是单形所在线性空间的全局最近点
// 0:这个最近点不是单形所在线性空间的全局最近点
// -1: 计算失败
int compute_neareat_point2simplex(double** ps,
    int back_dim,int len,double*p,double* lambda );




//l: t*n+p
// return lambda t1
// lambda*v1+(1-lambda)*v2
// t1*n+p
//
static inline void line_segment_nearest_2_line(double* l,double* v1,double *v2,
    double *lambda,double *t1)
{
    // solve min |Ax+b|

    double b[3]={v2[0]-l[3],v2[1]-l[4],v2[2]-l[5]};
    double A[3][2]={
        {v1[0]-v2[0],-l[0]},
        {v1[1]-v2[1],-l[1]},
        {v1[2]-v2[2],-l[2]}
    };
    double B[2][2] = {0};

// 计算 A^T * A
    for (int i = 0; i < 2; ++i) {
        for (int j = 0; j <= i; ++j)
        {
            for (int k = 0; k < 3; ++k)
            {
                B[i][j] += A[k][i] * A[k][j];
            }
            B[j][i] = B[i][j];
        }
    }
    double det=B[0][0]*B[1][1]-B[0][1]*B[1][0];
    if(double_equal_zero(det))
    {
        *lambda=1;*t1=-(A[0][0]*b[0]+A[1][0]*b[1]+A[2][0]*b[2]+B[0][0])/B[0][1];
        return ;
    }
    double B_inverse[2][2]={
        {B[1][1],-B[0][1]},
        {-B[1][0],B[0][0]}
    };
    double Ab[2]={A[0][0]*b[0]+A[1][0]*b[1]+A[2][0]*b[2],
        A[0][1]*b[0]+A[1][1]*b[1]+A[2][1]*b[2]
    };

    *lambda=-(Ab[0]*B_inverse[0][0]+Ab[1]*B_inverse[0][1])/det;
    *t1=-(Ab[0]*B_inverse[1][0]+Ab[1]*B_inverse[1][1])/det;
    if(*lambda>1){*lambda=1;
        *t1 = l[0] * (v1[0] - l[3]) +
            l[1] * (v1[1] - l[4]) +
            l[2] * (v1[2] - l[5]);
    }
    else if(*lambda<0){*lambda=0;
        *t1 = l[0] * (v2[0] - l[3]) +
            l[1] * (v2[1] - l[4]) +
            l[2] * (v2[2] - l[5]);
    }
}




//像下面这种询问必须设置阙值

//判断(直线和线段在投影平面内)直线和线段是否相交
// 这种询问必须设置阙值

//@ l: the normalized line n1*t1+p1
//0: no intersecting
//1 intersecting
//2 intersecting and parallel
// int is_line_intersecting_with_line_segment(double* l,double* v1,double *v2,double* t);

//判断三维空间点在线段中
// 这种询问必须设置阙值
// int is_point_in_line_segment(double* p,double *v1,double* v2);

//
// 判断在三维空间两线段（在投影平面内）是否相交
// 这种询问必须设置阙值
// int is_line_segment_intersecting_with_line_segment(double * v1,double* v2,
//     double *vv1,double*vv2,double* p);


//
// 避免询问，直接用计算代替，也就是类似神经网络的判断，数值判断，代替逻辑判断。
//

// @l :: t*n+p
// @poly::polygon is convex
// 求的是多边形边界和直线的交点，两个最近交点，排名最靠前的交点
// @return result[2][4]: id ,lambda,t,distance
//
void line_intersection_with_polygon(double* l,double** poly ,int len,
    double result[][4]);


//
// 返回在poly平面的直线截取的多边形
// 右手，法向n穿过右手掌心，大拇指方向即为多边形要提取的方向
/**/
void get_half_section_polygon_and_line3d(double** poly,int len,
    double*l,double** re,int* len1);


// 提取poly在p的法向切割部分
//两个polygon(凸的)顺序同向,且必须是逆时针
void  get_half_section_polygon_and_plane3d(double** poly,int len,
    double*p,double** re,int *len1);

// convex polygons


// 不管 poly1 poly2 是不是在一个平面，返回的是相交区域，可以是null,点，线，面
// 保证poly1和poly2是凸的

void get_intersections_from_two_polygons(double**poly1,int len1,
    double**poly2 ,int len2,double**result,int* len);

//
//return the triangle coordinate of p in p1 p2 p3
//

int get_triangle_coordinate(double* p,double * p1,double*p2,double* p3,
    double *re);

double compute_area_of_triangle(double* tri1,double* tri2,double* tri3);





// // return new polygon
// //
// //点的分量是2
// //poly必须是凸的,凹的不保证结果正确,虽然凹的有时候也会计算正确
//  //
// double** get_half_section_polygon_and_line(double** poly,int len,int *len1 ,double*l,int* tuihua);

// //返回polygon
// //
// //
// //求处在同一平面的两个多边形的相交区域。点的分量是2
// ///两个polygon(凸的)顺序同向,且必须是逆时针

// double** get_intersection_polygon_and_polygon(double** poly,int len,double** poly1,int len1,int* len2,int*tuihua);


//
//n1->n2 获得旋转变换
//
double * get_one_dir_trans2dir_rota_matrix(double* n1,double*n2);

//p 和polygon在同一平面
//
double * get_neareast_point2polygon2d(double** poly,int len,double* p);

//
//p和polygon不再同一平面
double * get_neareast_point2polygon3d(double** poly,int len,double* p);


#ifdef __cplusplus
}
#endif

#endif
